Program Listing for File CameraPoseVisualization.h

↰ Return to documentation for file (codes/pose_graph/utility/CameraPoseVisualization.h)

/*******************************************************
 * Copyright (C) 2019, Robotics Group, Nanyang Technology University
 *
 * \file CameraPoseVisualization.h
 * \author Zhang Handuo (hzhang032@e.ntu.edu.sg)
 * \date March 2017
 * \brief Camera visualization for RVIZ.
 *
 * Licensed under the GNU General Public License v3.0;
 * you may not use this file except in compliance with the License.
 *
 *******************************************************/

#pragma once

#include <ros/ros.h>
#include <std_msgs/ColorRGBA.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <opencv2/opencv.hpp>
#include "../parameters.h"

namespace pose_graph {
    class CameraPoseVisualization {
    public:
        #ifndef DOXYGEN_SHOULD_SKIP_THIS
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
#endif /* DOXYGEN_SHOULD_SKIP_THIS */
        std::string m_marker_ns;

        CameraPoseVisualization();

        CameraPoseVisualization(float r, float g, float b, float a);

        void setImageBoundaryColor(float r, float g, float b, float a = 1.0);

        void setOpticalCenterConnectorColor(float r, float g, float b, float a = 1.0);

        void setScale(double s);

        void setLineWidth(double width);

        void add_pose(const Eigen::Vector3d &p, const Eigen::Quaterniond &q);

        void reset();

        void publish_by(ros::Publisher &pub, const std_msgs::Header &header);

        void add_edge(const Eigen::Vector3d &p0, const Eigen::Vector3d &p1);

        void add_loopedge(const Eigen::Vector3d &p0, const Eigen::Vector3d &p1);

        //void add_image(const Eigen::Vector3d& T, const Eigen::Matrix3d& R, const cv::Mat &src);
        void publish_image_by(ros::Publisher &pub, const std_msgs::Header &header);

    private:
        std::vector<visualization_msgs::Marker> m_markers;
        std_msgs::ColorRGBA m_image_boundary_color;
        std_msgs::ColorRGBA m_optical_center_connector_color;
        double m_scale;
        double m_line_width;
        visualization_msgs::Marker image;
        int LOOP_EDGE_NUM;
        int tmp_loop_edge_num;

        static const Eigen::Vector3d imlt;
        static const Eigen::Vector3d imlb;
        static const Eigen::Vector3d imrt;
        static const Eigen::Vector3d imrb;
        static const Eigen::Vector3d oc;
        static const Eigen::Vector3d lt0;
        static const Eigen::Vector3d lt1;
        static const Eigen::Vector3d lt2;
    };
}